/*
*  Matrix4.cpp
*
*  Created on: Jan 14, 2012
*  
*/

#include  "Matrix4.h"
#include <math.h>

namespace Physics
{
	Matrix4::Matrix4()
	{
		xx, xy, xz, xw = 0.0f;
		yx, yy, yz, yw = 0.0f;
		zx, zy, zz, zw = 0.0f;
		wx, wy, wz, ww = 0.0f;
	}

	Matrix4::Matrix4( float XX, float XY, float XZ, float XW, 
					  float YX, float YY, float YZ, float YW,
					  float ZX, float ZY, float ZZ, float ZW,
					  float WX, float WY, float WZ, float WW)
	{
		xx = XX; xy = XY; xz = XZ; xw = XW;
		yx = YX; yy = YY; yz = YZ; yw = YW;
		zx = ZX; zy = ZY; zz = ZZ; zw = ZW; 
		wx = WX; wy = WY; wz = WZ; ww = WW;
	}

	Matrix4::~Matrix4()
	{
		
	}

	// figure out a new approach to this....might have to code a [] operator
	float Matrix4::getIndex(int row, int col)
	{
		float value = 0.0f;

		switch(row)
		{
		case 0:

			switch(col)
			{
				case 0:
					value = xx;
					break;

				case 1:
					value = xy;
					break;

				case 2:
					value = xz;
					break;

				case 3:
					value = xw;
					break;

				default:
					value = 0.0f;
					break;
			}
			
			break;

		case 1:

			switch(col)
			{
				case 0:
					value = yx;
					break;

				case 1:
					value = yy;
					break;

				case 2:
					value = yz;
					break;

				case 3:
					value = yw;
					break;

				default:
					value = 0.0f;
					break;

			}
			
			break;

		case 2:

			switch(col)
			{
				case 0:
					value = zx;
					break;

				case 1:
					value = zy;
					break;

				case 2:
					value = zz;
					break;

				case 3:
					value = zw;
					break;

				default:
					value = 0.0f;
					break;

			}
			
			break;

		case 3:

			switch(col)
			{
				case 0:
					value = wx;
					break;

				case 1:
					value = wy;
					break;

				case 2:
					value = wz;
					break;

				case 3:
					value = ww;
					break;

				default:
					value = 0.0f;
					break;

			}
			
			break;

		default:
			value = 0.0f;
			break;

		}

		return value;
	}

	void Matrix4::identity()
	{
		xx = yy = zz = ww = 1.0f;

		xy = xz = xw = yx = 0.0f;
		yz = yw = zx = zy = 0.0f;
		zw = wx = wy = wz = 0.0f;
	}

	Matrix4 Matrix4::operator +(Matrix4 &matrix)
	{
		return Matrix4(xx + matrix.getIndex(0,0), xy + matrix.getIndex(0,1), xz + matrix.getIndex(0,2), xw + matrix.getIndex(0,3),
					   yx + matrix.getIndex(1,0), yy + matrix.getIndex(1,1), yz + matrix.getIndex(1,2), yw + matrix.getIndex(1,3),
					   zx + matrix.getIndex(2,0), zy + matrix.getIndex(2,1), zz + matrix.getIndex(3,2), zw + matrix.getIndex(2,3),
					   wx + matrix.getIndex(3,0), wy + matrix.getIndex(3,1), wz + matrix.getIndex(3,2), ww + matrix.getIndex(3,3));

	}

	Matrix4 Matrix4::operator -(Matrix4 &matrix)
	{
		return Matrix4(xx - matrix.getIndex(0,0), xy - matrix.getIndex(0,1), xz - matrix.getIndex(0,2), xw - matrix.getIndex(0,3),
					   yx - matrix.getIndex(1,0), yy - matrix.getIndex(1,1), yz - matrix.getIndex(1,2), yw - matrix.getIndex(1,3),
					   zx - matrix.getIndex(2,0), zy - matrix.getIndex(2,1), zz - matrix.getIndex(3,2), zw - matrix.getIndex(2,3),
					   wx - matrix.getIndex(3,0), wy - matrix.getIndex(3,1), wz - matrix.getIndex(3,2), ww - matrix.getIndex(3,3));
	}
	
	// Had an issue when programming it. Each calculation is correct,
	// but when making the matrix to return it it had issues
	Matrix4 Matrix4::operator *(Matrix4 &matrix)
	{	
		Matrix4 result( 
						(xx * matrix.getIndex(0,0)) + (xy * matrix.getIndex(1,0)) + (xz * matrix.getIndex(2,0)) + (xw * matrix.getIndex(3,0)),
						(xx * matrix.getIndex(0,1)) + (xy * matrix.getIndex(1,1)) + (xz * matrix.getIndex(2,1)) + (xw * matrix.getIndex(3,1)),
						(xx * matrix.getIndex(0,2)) + (xy * matrix.getIndex(1,2)) + (xz * matrix.getIndex(2,2)) + (xw * matrix.getIndex(3,2)),
						(xx * matrix.getIndex(0,3)) + (xy * matrix.getIndex(1,3)) + (xz * matrix.getIndex(2,3)) + (xw * matrix.getIndex(3,3)),
	
						(yx * matrix.getIndex(0,0)) + (yy * matrix.getIndex(1,0)) + (yz * matrix.getIndex(2,0)) + (yw * matrix.getIndex(3,0)),
						(yx * matrix.getIndex(0,1)) + (yy * matrix.getIndex(1,1)) + (yz * matrix.getIndex(2,1)) + (yw * matrix.getIndex(3,1)),
						(yx * matrix.getIndex(0,2)) + (yy * matrix.getIndex(1,2)) + (yz * matrix.getIndex(2,2)) + (yw * matrix.getIndex(3,2)),
						(yx * matrix.getIndex(0,3)) + (yy * matrix.getIndex(1,3)) + (yz * matrix.getIndex(2,3)) + (yw * matrix.getIndex(3,3)),
	
						(zx * matrix.getIndex(0,0)) + (zy * matrix.getIndex(1,0)) + (zz * matrix.getIndex(2,0)) + (zw * matrix.getIndex(3,0)),
						(zx * matrix.getIndex(0,1)) + (zy * matrix.getIndex(1,1)) + (zz * matrix.getIndex(2,1)) + (zw * matrix.getIndex(3,1)),
						(zx * matrix.getIndex(0,2)) + (zy * matrix.getIndex(1,2)) + (zz * matrix.getIndex(2,2)) + (zw * matrix.getIndex(3,2)),
						(zx * matrix.getIndex(0,3)) + (zy * matrix.getIndex(1,3)) + (zz * matrix.getIndex(2,3)) + (zw * matrix.getIndex(3,3)),
		
						(wx * matrix.getIndex(0,0)) + (wy * matrix.getIndex(1,0)) + (wz * matrix.getIndex(2,0)) + (ww * matrix.getIndex(3,0)),
						(wx * matrix.getIndex(0,1)) + (wy * matrix.getIndex(1,1)) + (wz * matrix.getIndex(2,1)) + (ww * matrix.getIndex(3,1)),
						(wx * matrix.getIndex(0,2)) + (wy * matrix.getIndex(1,2)) + (wz * matrix.getIndex(2,2)) + (ww * matrix.getIndex(3,2)),
						(wx * matrix.getIndex(0,3)) + (wy * matrix.getIndex(1,3)) + (wz * matrix.getIndex(2,3)) + (ww * matrix.getIndex(3,3))
						
						);

		return result;

	}
		 
	void Matrix4::Scale(float scaleX, float scaleY, float scaleZ)
	{
		xx *= scaleX; xy *= scaleX; xz *= scaleX; xw *= scaleX;
		yx *= scaleY; yy *= scaleY; yz *= scaleY; yw *= scaleY;
		zx *= scaleZ; zy *= scaleZ; zz *= scaleZ; zw *= scaleZ;
		wx *= scaleX; wy *= scaleY; wz *= scaleZ; ww = 1.0f; 				
	}

	void Matrix4::Scale(float scaleAmount)
	{
		Scale(scaleAmount, scaleAmount, scaleAmount);
	}

	void Matrix4::Translate(float translateX, float translateY, float translateZ)
	{
		xx = yy = zz = ww = 1.0f;
		xy = xz = xw = 0.0f;
		yx = yz = yw = 0.0f;
		zx = zy = zw = 0.0f;

		wx = (wx + translateX);
		wy = (wy + translateY);
		wz = (wz + translateZ);
	}

	void Matrix4::RotateX(float theta)
	{
		float cos = cosf(theta);
		float sin = sinf(theta);

		xx = ww = 1.0f;
		yy = zz = cos;
		
		xy = xz = 0.0f;
		xw = yx = 0.0f;
		yw = zx = 0.0f;
		zw = wx = 0.0f;
		wy = wz = 0.0f;
		
		yz = -sin;
		zy = sin;
	}

	void Matrix4::RotateY(float theta)
	{
		float cos = cosf(theta);
		float sin = sinf(theta);

		xx = zz = cos;

		xz = -sin;
		zx = sin;
		
		yy = ww = 1.0f;
		
		xy = xw = 0.0f;
		yx = yz = 0.0f;
		yw = zy = 0.0f;
		zw = wx = 0.0f;
		wy = wz = 0.0f;
	}

	void Matrix4::RotateZ(float theta)
	{
		float cos = cosf(theta);
		float sin = sinf(theta);

		xx = yy = cos;
		xy = -sin;
		yx = sin;

		zz = ww = 1.0f;
		
		xz = xw = 0.0f;
		yz = yw = 0.0f;
		zx = zy = 0.0f;
		zw = wx = 0.0f;
		wy = wz = 0.0f;
	}
} /* NAMESPACE */